#include "ImageProcessWorker.h"
#include <fstream>


void savePointCloudToFile(const cv::Mat& mat, const std::string& filename) {
	std::ofstream file(filename);
	if (!file.is_open()) {
		qCritical() << "无法打开文件:" << QString::fromStdString(filename);
		return;
	}

	// 遍历矩阵的每个元素并写入文件
	for (int i = 0; i < mat.rows; ++i) {
		for (int j = 0; j < mat.cols; ++j) {
			file << mat.at<float>(i, j); // 假设 mat 为 CV_32F 类型
			if (j < mat.cols - 1) {
				file << " "; // 列间空格
			}
		}
		file << "\n"; // 行间换行
	}

	file.close();
	qDebug() << "点云数据已保存到文件:" << QString::fromStdString(filename);
}


// ImageProcessConfig 图像处理的信息 
ImageProcessWorker::ImageProcessWorker(int rows, int cols, const ImageProcessConfig& imageProCig, const caliParam& calibration, QObject* parent)
	: QObject(parent) { 
	// 初始化最后结果Mat
	m_result = cv::Mat::zeros(rows, cols, CV_32FC1);
	m_imgproconfig = imageProCig;
	m_calibration = calibration;

	m_cam_intrinsic = (cv::Mat_<double>(3, 3) << calibration.CameraMatrix.at(0), calibration.CameraMatrix.at(1), calibration.CameraMatrix.at(2),
		calibration.CameraMatrix.at(3), calibration.CameraMatrix.at(4), calibration.CameraMatrix.at(5),
		calibration.CameraMatrix.at(6), calibration.CameraMatrix.at(7), calibration.CameraMatrix.at(8)); // 示例内参数矩阵
	m_cam_discoeffs = (cv::Mat_<double>(1, 5) << calibration.disCoeffs.at(0), calibration.disCoeffs.at(1), calibration.disCoeffs.at(2),
		calibration.disCoeffs.at(3), calibration.disCoeffs.at(4));
	m_step= (cv::Mat_<double>(1, 3) << calibration.Step.at(0), calibration.Step.at(1), calibration.Step.at(2));

	YD = imageProCig.YD;
}

ImageProcessWorker::~ImageProcessWorker() { }

void ImageProcessWorker::processFrameData(QThreadPool& threadPool, int num, cv::Mat& frame) { 
	// 创建一个新的 Worker 对象将其传给 threadPool 以启动任务，threadPool会接管 Worker 对象的生命周期管理，不需手动删除 Worker 对象
	//qDebug() << "创建worker 对象*****************";
	//threadPool.start(new Worker(num, frame, this));
	QThreadPool::globalInstance()->start(new Worker(num, frame, this));
	
	// 预约一个线程, 增加活动线程的数量
	//threadPool.reserveThread();
}

void ImageProcessWorker::addProcessResult(int num, vector<float>& result) {
	QMutexLocker locker(&m_mutex);
	// 如果 result 的大小超过了 m_result 的当前行数，调整 m_result 的大小
	if (result.size() > m_result.rows) {
		cv::Mat newResult(result.size(), m_result.cols, CV_32F, cv::Scalar(0.0f));
		m_result.copyTo(newResult(cv::Rect(0, 0, m_result.cols, m_result.rows)));
		m_result = newResult;
	}

	// 每次处理的 result 数据作为一列，填充到 m_result 中
	qDebug() << QString("正在填充第%1列数据").arg(num);
	for (int row = 0; row < result.size(); row++) {
		m_result.at<float>(row, num) = result[row];
	}

	// 将剩余的行填充为 0（如果 result.size() 小于 m_result.rows）
	for (int row = result.size(); row < m_result.rows; row++) {
		m_result.at<float>(row, num) = 0.0f;
	}
	qDebug() << "当前 m_processCount" << m_processCount;
	m_processCount++;
	// 若处理的图像数量达到预定值，通知主线程处理完成
	if (m_processCount == m_result.cols-5) {
		cv::imwrite("m_result.jpg", m_result);
		// 截取ROI（0,0,宽，高930）	
		cv::Rect roi(0, 0, m_result.cols-5, 930); // ROI区域 (x, y, width, height)
		cv::Mat roiImg = m_result(roi);
		cv::imwrite("m_result_ROI.jpg", roiImg);
		qDebug() << "*****************m_result.cols:" << roiImg.cols << "m_result.rows:" << roiImg.rows;
		// 转换为点云数据
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

		for (int y = 0; y < roiImg.rows; y++) {
			for (int x = 0; x < roiImg.cols; x++) {
				// 获取深度值
				float depth = roiImg.at<float>(y, x);
				pcl::PointXYZ point;
				point.x = static_cast<float>(x);           // 使用图像的像素位置作为 x 坐标
				point.y = static_cast<float>(y);           // 使用图像的像素位置作为 y 坐标
				point.z = depth/10;                         // 使用深度值作为 z 坐标
				cloud->points.push_back(point);
			}
		}

		// 设置点云尺寸和结构
		cloud->width = roiImg.cols;
		cloud->height = roiImg.rows;
		cloud->is_dense = false;

		/*qDebug() << "************当前点云大小:" << cloud->points.size();
		qDebug() << "保存点云数据到txt";
		savePointCloudToFile(m_result, "pointcloud_MatData.txt");*/
		qDebug() << "************点云数据处理完成，上传到主线程";
		//emit processFinished(cloud);
	}
}

void ImageProcessWorker::calculateXZ(const vector<double>& aCoeffs, const vector<double>& bCoeffs, float u, float v, float& X, float& Z)
{
	// 计算 X
	X = aCoeffs[0] * u * u * u + aCoeffs[1] * v * v * v +
		aCoeffs[2] * u * u * v + aCoeffs[3] * u * v * v +
		aCoeffs[4] * u * u + aCoeffs[5] * v * v +
		aCoeffs[6] * u * v + aCoeffs[7] * u +
		aCoeffs[8] * v + aCoeffs[9];

	// 计算 Z
	Z = bCoeffs[0] * u * u * u + bCoeffs[1] * v * v * v +
		bCoeffs[2] * u * u * v + bCoeffs[3] * u * v * v +
		bCoeffs[4] * u * u + bCoeffs[5] * v * v +
		bCoeffs[6] * u * v + bCoeffs[7] * u +
		bCoeffs[8] * v + bCoeffs[9];
}

void ImageProcessWorker::getReconstructionData(int num, vector<cv::Point2f>& results)
{
	all_center_points.push_back(results);
	//qDebug() << "当前 m_processCount" << m_processCount;
	m_processCount++;
	map_id_line_pixels.insert(pair<int, vector<cv::Point2f>>(num, results));
	if (m_processCount == m_result.cols-5)
	{
		vector<vector<cv::Point3f>> all_center_points_3d;
		if (all_center_points.size() > 0)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTemp(new pcl::PointCloud<pcl::PointXYZ>);
			for (auto& pt : map_id_line_pixels)
			{
				qDebug() << "pt.first-1=" << pt.first - 1;
				float maxZval = 0.0;
				vector<cv::Point3f>tempRowPts;
				for (auto& p: pt.second)
				{
					cv::Point3f point;

					//----------------------------传统光平面标定--三维重建
					//cv::Point3f unit_ray = pixelToUnitRay(p, m_cam_intrinsic);
					//double s = -m_calibration.PlaneParam[3] / (unit_ray.x * m_calibration.PlaneParam[0] + unit_ray.y * m_calibration.PlaneParam[1] + unit_ray.z * m_calibration.PlaneParam[2]);
					//point.x = s * unit_ray.x + (pt.first - 1) * m_step.at<double>(0);
					//point.y = s * unit_ray.y + (pt.first - 1) * m_step.at<double>(1);//pt.first - 1
					//point.z = s * unit_ray.z + 1 * m_step.at<double>(2);

					//---------------------------单映变换标定--三维重建
					float X = 0.0, Z = 0.0;
					calculateXZ(m_imgproconfig.aCoefficients, m_imgproconfig.bCoefficients, p.x, p.y, X, Z);
					point.x = X;
					point.y = (pt.first - 1) * YD;
					point.z = Z;
					if (point.z > maxZval)
					{
                        maxZval = point.z;
					}
					tempRowPts.push_back(point);
					//计算出来的点保存到点云文件中
					cloud->points.push_back(pcl::PointXYZ(point.x, point.y, point.z));

				}
				for (auto& p : tempRowPts)
				{
					cloudTemp->points.push_back(pcl::PointXYZ(p.x, p.y, maxZval-p.z));
				}
				//收集所有激光线中心点三维数据
				all_center_points_3d.push_back(tempRowPts);
				
			}
			cv::Mat heightGrayImg = convertToHeightMap(all_center_points_3d);
			cv::imwrite("./heightGrayImg.png", heightGrayImg);
			// 设置点云尺寸和结构
			cloud->width = 1;
			cloud->height = cloud->points.size();


			cloudTemp->width = 1;
            cloudTemp->height = cloudTemp->points.size();
			//cloud->is_dense = false;
			pcl::io::savePCDFile("./cloud.pcd", *cloud);
            pcl::io::savePCDFile("./cloudTemp.pcd", *cloudTemp);
			qDebug() << "************当前点云大小:" << cloud->points.size();
			
			//----点云滤波处理
			this->cloudFilter(cloud);

			//-----点云数据统计参数计算
			staticalParameters cloudStaticData;
			this->calculateZStatistics(cloud, cloudStaticData);

			emit processFinished(cloud, cloudStaticData);
			qDebug() << "保存点云数据到txt";
			//savePointCloudToFile(m_result, "pointcloud_MatData.txt");
			qDebug() << "************点云数据处理完成，上传到主线程";

			//初始化计数变量8
			all_center_points.clear();
			m_processCount = 0;
			map_id_line_pixels.clear();
		}
	}
}

const MyError& ImageProcessWorker::getError() const {
	return m_error;
}

cv::Point3f ImageProcessWorker::pixelToUnitRay(const cv::Point2f& pixel, const cv::Mat& intrinsic)
{
	/*
		   |u|   |fx 0 cx||Xc/Zc|
		 Zc|v| = |0 fy cy||Yc/Zc|
		   |1|   |0  0  1||  1  |
	  */
	const double xd = pixel.x - intrinsic.at<double>(0, 2), yd = pixel.y - intrinsic.at<double>(1, 2);
	cv::Point3f point(xd / intrinsic.at<double>(0, 0), yd / intrinsic.at<double>(1, 1), 1.);
	const double norm = std::sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
	return cv::Point3f(point.x / norm, point.y / norm, point.z / norm);
}

void ImageProcessWorker::cloudFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
	//体素滤波
	pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;  // 创建体素滤波对象
	voxel_grid.setInputCloud(cloud);           // 设置输入点云
	voxel_grid.setLeafSize(0.15f, 0.15f, 0.15f); // 设置体素网格的大小
	pcl::PointCloud<pcl::PointXYZ>::Ptr resampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	voxel_grid.filter(*resampled_cloud);       // 执行体素滤波，进行点云重采样
	pcl::io::savePCDFile("./resampled_cloud.pcd", *resampled_cloud); // 保存重采样后的点云数据

	// 统计滤波
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; // 创建统计滤波对象
	sor.setInputCloud(resampled_cloud);                         // 设置输入点云
	sor.setMeanK(10);                                 // 设置每个点的临近数量
	sor.setStddevMulThresh(1.0);                      // 设置标准差倍数
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	sor.filter(*cloud_filtered);                       // 执行统计滤波，去除离群点
	pcl::io::savePCDFile("./filter_cloud.pcd", *cloud_filtered); // 保存滤波后的点云数据
	cloud = cloud_filtered;
	pointCloudToHeightMap(cloud_filtered, 1);
}

void ImageProcessWorker::calculateZStatistics(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, staticalParameters& param)
{
	//计算点云的质心坐标
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	param.meanZ = centroid[2];

	//计算点云 最大值 最小值
	pcl::PointXYZ min_pt, max_pt;
	pcl::getMinMax3D(*cloud, min_pt, max_pt);
	param.minZ = min_pt.z;
	param.maxZ = max_pt.z;

	//计算标准差
	float variance = 0.0;
	for (auto& point : *cloud)
	{
		float diff = point.z - param.meanZ;
		variance += diff * diff;
	}
	variance /= cloud->size();
	param.stdZ = sqrt(variance);
}

cv::Mat ImageProcessWorker::pointCloudToHeightMap(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float resolution, bool use_max_height)
{
	try {
		if (cloud == nullptr || cloud->empty()) {
			return cv::Mat();
		}

		// 1. 获取点云边界和尺寸（单位：米）
		pcl::PointXYZ min_pt, max_pt;
		pcl::getMinMax3D(*cloud, min_pt, max_pt);
		float width = max_pt.x - min_pt.x;
		float height = max_pt.y - min_pt.y;

		// 2. 自动计算最佳分辨率（避免空白行）
		float avg_spacing = (width + height) / (2 * cloud->size());
		const float resolution = std::max(avg_spacing * 0.8f, 0.4f); // 最小0.001m(1mm)

		// 3. 计算图像尺寸
		int cols = static_cast<int>(width / resolution) + 1;
		int rows = static_cast<int>(height / resolution) + 1;

		// 安全检查
		if (resolution <= 0 || cols <= 0 || rows <= 0 || cols > 10000 || rows > 10000) {
			std::cerr << "Invalid resolution or image size: "
				<< resolution << ", " << cols << "x" << rows << std::endl;
			return cv::Mat();
		}

		// 4. 创建并初始化高度图
		cv::Mat height_map(rows, cols, CV_32FC1, cv::Scalar(NAN));
		float min_z = min_pt.z, max_z = max_pt.z;

		// 5. 填充高度值（带插值补偿）
		std::vector<std::vector<float>> z_buffer(cols, std::vector<float>(rows, NAN));

		// 第一遍：原始数据填充
		for (const auto& point : *cloud) {
			int col = static_cast<int>((point.x - min_pt.x) / resolution);
			int row = static_cast<int>((point.y - min_pt.y) / resolution);

			if (col >= 0 && col < cols && row >= 0 && row < rows) {
				float& val = z_buffer[col][row];
				val = std::isnan(val) ? point.z :
					(use_max_height ? std::max(val, point.z) : std::min(val, point.z));
			}
		}

		// 第二遍：简单线性插值填充空白
		for (int c = 0; c < cols; ++c) {
			float last_valid = min_z;
			for (int r = 0; r < rows; ++r) {
				if (!std::isnan(z_buffer[c][r])) {
					last_valid = z_buffer[c][r];
				}
				else if (r > 0 && !std::isnan(z_buffer[c][r - 1])) {
					z_buffer[c][r] = last_valid; // 垂直方向插值
				}
			}
		}

		// 转换到OpenCV Mat
		for (int r = 0; r < rows; ++r) {
			for (int c = 0; c < cols; ++c) {
				height_map.at<float>(r, c) = std::isnan(z_buffer[c][r]) ? min_z : z_buffer[c][r];
			}
		}

		// 6. 增强对比度归一化
		cv::Mat gray_image;
		cv::normalize(height_map, gray_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);

		// 7. 后处理：高斯模糊消除锯齿
		cv::GaussianBlur(gray_image, gray_image, cv::Size(3, 3), 0.5);
		qDebug() << "开始保存图片......";
		cv::imwrite("./heightMap.bmp", gray_image);
		qDebug() << "保存图片完成！！！！！！！！";
		return gray_image;
	}
	catch (const std::exception& e) {
		std::cerr << "Error: " << e.what() << std::endl;
		return cv::Mat();
	}
}

cv::Mat ImageProcessWorker::convertToHeightMap(const std::vector<std::vector<cv::Point3f>>& all_center_points)
{
	// 1. 确定矩阵尺寸（rows=外层vector大小，cols=最长子vector长度）
	int rows = static_cast<int>(all_center_points.size());
	int cols = 0;

	// 找到最大子vector长度作为列数
	for (const auto& vec : all_center_points) {
		cols = std::max(cols, static_cast<int>(vec.size()));
	}

	// 2. 创建矩阵并初始化（默认填充0）
	cv::Mat height_map(rows, cols, CV_32FC1, cv::Scalar(0));

	// 3. 收集所有Z值用于归一化
	float min_z = FLT_MAX, max_z = -FLT_MAX;
	for (const auto& vec : all_center_points) {
		for (const auto& point : vec) {
			min_z = std::min(min_z, point.z);
			max_z = std::max(max_z, point.z);
		}
	}
	float range = max_z - min_z;
	if (range < 1e-6f) range = 1.0f; // 防止除零

	// 4. 填充数据（按行优先存储）
	for (int r = 0; r < rows; ++r) {
		const auto& vec = all_center_points[r];
		for (int c = 0; c < vec.size(); ++c) {
			// 归一化Z值到0-255
			float normalized = 255 * (vec[c].z - min_z) / range;
			height_map.at<float>(r, c) = normalized;
		}
		// 当前行剩余部分保持为0（黑色）
	}

	// 5. 转换为8位灰度图
	cv::Mat gray_image;
	height_map.convertTo(gray_image, CV_8UC1);

	return gray_image;
}


////////////////////////////// ImageProcessWorker::Worker ///////////////////////////////////
ImageProcessWorker::Worker::Worker(int num, cv::Mat& frame, ImageProcessWorker* parent) 
	: m_parent(parent), m_num(num) {
		setAutoDelete(true);
		m_frame = frame.clone();
		m_inter_imgproconfig = m_parent->m_imgproconfig;
}

ImageProcessWorker::Worker::~Worker() {
	if (!m_frame.empty()) {
		m_frame.release();
	}
}

void ImageProcessWorker::Worker::run() {
	vector<float> values;         
	try {
		// 执行图像处理
		qDebug() << "正在处理第 " << m_num << " 张激光图像...";
		// 测试lee 保存图像到本地
		//cv::imwrite("D:/A_LEE_WORK/Pro12_Flatness/image_store/image_" + std::to_string(m_num) + ".jpg", m_frame);
		////// 提取图像中心点
		//std::vector<cv::Vec2f> center_points;
		//ImageUtils::findCenterPoint(m_num, m_frame, 25, center_points);
		//// 计算最高点x坐标
		//int highestX = -1;
		//for (const auto& point : center_points) {
		//	if (point[1] > highestX) {
		//		highestX = point[1];
		//	}
		//}
		//// 计算每一点的实际高度差
		//for (auto& point : center_points) {
		//	int deltaX = static_cast<int>(highestX - point[1]);
		//	float deltaX2real = deltaX * m_inter_imgproconfig.pixelSize;
		//	float realHeight = m_inter_imgproconfig.poleLength * 2 * deltaX / (m_inter_imgproconfig.focalLength + deltaX);
		//	// 添加高度信息
		//	values.push_back(realHeight);
		//}
		//QThread::msleep(100);
		//// 更新结果 (当前帧的激光三维数据)
		//qDebug() << QString("*****************************第%1激光线高度数据长度为: %2").arg(m_num).arg(values.size());
		//m_parent->addProcessResult(m_num, values);
		//新方法获取激光线中心坐标

		qDebug() << "进入三维重建函数";
		vector<cv::Point2f>center_points;
		center_points = ImageUtils::GrayScaleCenter(ImageUtils::downwardThreFileter(m_frame, 3));
		//center_points = ImageUtils::simpleMaxValueMethod(m_frame);
		
		//vector<cv::Point2f>interpolatedPoints = ImageUtils::interpolatePoints(center_points, 1);//线性插值
		m_parent->getReconstructionData(m_num, center_points);

	} catch (...) {
		qCritical() << "#Error...";
	}
}
